﻿#pragma once

#include <learn/alg/alg_geom.h>
#include <learn/geom/geom_bezier_curve.h>
#include <learn/geom/geom_bspline_curve.h>
#include <learn/geom/geom_bspline_surface.h>
#include <learn/geom/quat.h>
#include <learn/math/math.h>
#include <learn/math/matrix.h>
#include <learn/math/matrix_solver.h>
#include <learn/math/polynomial.h>

#include <functional>
#include <vector>

namespace xi
{

namespace alg
{

using namespace xi::math;
using namespace xi::geom;

template <class T, class Real = T> class Alg_Newton_Interpolator
{
    static constexpr auto cmp = Compare{};

  public:
    explicit Alg_Newton_Interpolator() = default;

    /**
     * @brief 构造插值条件
     *
     * @param[in] x
     * @param[in] f
     */
    Alg_Newton_Interpolator(const std::vector<Real> &x, const std::vector<T> &f)
    {
        std::size_t n = f.size();
        for (std::size_t i = 0; i < n; i++)
            insert(x[i], f[i]);
    }

    /**
     * @brief 获得插值多项式
     *
     * @return Polynomial<T, Real>
     */
    Polynomial<T, Real> interpolate() const
    {
        std::size_t n = m_table.size();
        Polynomial<T, Real> poly(0);
        Polynomial<T, Real> tmp(0);
        poly.coefficient(0) = m_table[n - 1].back();
        tmp.coefficient(0) = T{1};
        for (int i = n - 2; i >= 0; i--)
        {
            tmp *= Polynomial<T, Real>({T{-m_x[i + 1]}, T{1}});
            poly += tmp * m_table[i].back();
        }
        return poly;
    }

    /**
     * @brief 直接求值
     *
     * @param[in] t
     * @return T
     */
    T value(Real t) const
    {
        T result = T{};
        T diff = T{1};
        std::size_t n = m_table.size();
        for (int i = n - 1; i >= 0; i--)
        {
            result += diff * m_table[i].back();
            diff *= t - m_x[i];
        }
        return result;
    }

    /**
     * @brief 插入数据
     *
     * @param[in] x
     * @param[in] f
     */
    void insert(Real x, T f)
    {
        static std::size_t count = 1;

        // count 记录重数
        if (!m_x.empty() && cmp.test<Real>(x, m_x.back()))
            count++;
        else
            count = 1;

        m_x.push_back(x);
        m_f.push_back(f);

        m_table.emplace_back();
        for (std::size_t i = 1; i < count; i++)
        {
            std::size_t j = m_table.size() - i;
            m_table[j].push_back(m_table[j - 1][i - 1]);
        }

        m_table[m_table.size() - count].push_back(f / factorial(count - 1));
        for (std::size_t i = count; i < m_table.size(); i++)
        {
            std::size_t j = m_table.size() - i - 1;
            m_table[j].push_back((m_table[j + 1][i - 1] - m_table[j][i - 1]) / (m_x[j + i] - m_x[j]));
        }
    }

    /**
     * @brief neville_aitken 算法
     *
     * @param[in] x
     * @param[in] f
     * @param[in] t
     * @return T
     */
    static T neville_aitken(const std::vector<Real> &x, const std::vector<T> &f, Real t)
    {
        // 存放差分表
        std::size_t N = x.size();
        std::vector<std::vector<T>> table(N, std::vector<T>(N));

        // 从下向上计算
        for (int i = N - 1; i >= 0; i--)
        {
            table[i][0] = f[i];
            for (std::size_t j = 1; i + j < N; j++)
                table[i][j] = (table[i + 1][j - 1] * (t - x[i]) - table[i][j - 1] * (t - x[i + j])) / (x[i + j] - x[i]);
        }

        return table[0][N - 1];
    }

    /**
     * @brief 利用差分表递归计算
     *
     * @param[in] f
     * @param[in] n
     * @return T
     */
    static std::vector<T> recursion(const std::vector<T> &f, std::size_t n)
    {
        std::vector<std::vector<T>> table(f.size(), std::vector<T>(f.size() + n));
        std::copy(f.begin(), f.end(), table[0].begin());
        for (std::size_t i = 0; i < f.size(); i++)
        {
            for (int j = i - 1; j >= 0; j--)
                table[i - j][j] = table[i - j - 1][j + 1] - table[i - j - 1][j];
        }

        std::fill(table.back().begin(), table.back().end(), table.back().front());
        for (std::size_t i = 1; i < f.size(); i++)
        {
            for (std::size_t j = i + 1; j < i + 1 + n; j++)
                table[f.size() - i - 1][j] = table[f.size() - i][j - 1] + table[f.size() - i - 1][j - 1];
        }
        return table.front();
    }

  protected:
    std::vector<Real> m_x;               // 自变量
    std::vector<T> m_f;                  // 因变量
    std::vector<std::vector<T>> m_table; // 差分表
};

template <class T, class Real = T> class Alg_Barycentric_Interpolator
{
    static constexpr auto cmp = Compare{};

  public:
    explicit Alg_Barycentric_Interpolator() = default;

    /**
     * @brief 构造插值条件
     *
     * @param[in] x
     * @param[in] f
     */
    Alg_Barycentric_Interpolator(const std::vector<Real> &x, const std::vector<T> &f)
    {
        std::size_t n = f.size();
        for (std::size_t i = 0; i < n; i++)
            insert(x[i], f[i]);
    }

    /**
     * @brief 求值
     *
     * @param[in] t
     * @return T
     */
    T value(Real t) const
    {
        T result = T{};
        Real diff = Real{};
        for (std::size_t i = 0; i < m_x.size(); i++)
        {
            // 如果恰好取到节点，则直接返回节点值
            Real d = t - m_x[i];
            if (cmp.test<Real>(d, Real{}))
                return m_f[i];

            Real w = m_w[i] / d;
            result += m_f[i] * w;
            diff += w;
        }
        return result / diff;
    }

    /**
     * @brief 插入数据
     *
     * @param[in] x
     * @param[in] f
     */
    void insert(Real x, T f)
    {
        Real t = Real{1};
        for (std::size_t i = 0; i < m_x.size(); i++)
        {
            Real w = x - m_x[i];
            m_w[i] /= -w;
            t *= w;
        }
        m_w.push_back(Real{1} / t);
        m_x.push_back(x);
        m_f.push_back(f);
    }

    /**
     * @brief 在 [-1, 1] 区间内，均匀采样插值
     *
     * @param[in] f
     * @param[in] n
     * @return Alg_Barycentric_Interpolator
     */
    static Alg_Barycentric_Interpolator equidistant(std::function<T(Real)> f, std::size_t n)
    {
        Alg_Barycentric_Interpolator interp;

        interp.m_f.reserve(n);
        interp.m_x.reserve(n);
        interp.m_w.reserve(n);

        int sign = -1;
        for (std::size_t i = 0; i < n; i++)
        {
            sign *= -1;
            Real x = Real{-1.0 + 2.0 * i / (n - 1.0)};
            Real w = Real{1.0 * binomial(n - 1, i) * sign};
            interp.m_x.emplace_back(x);
            interp.m_f.emplace_back(f(x));
            interp.m_w.emplace_back(w);
        }
        return interp;
    }

    /**
     * @brief 第二类切比雪夫插值，在 [-1, 1] 区间内，包含端点且在附近密集采样，避免荣格现象
     *
     * @param[in] f
     * @param[in] n
     * @return Alg_Barycentric_Interpolator
     */
    static Alg_Barycentric_Interpolator chebyshev2(std::function<T(Real)> f, std::size_t n)
    {
        Alg_Barycentric_Interpolator interp;

        interp.m_f.reserve(n);
        interp.m_x.reserve(n);
        interp.m_w.reserve(n);

        int sign = -1;
        for (std::size_t i = 0; i < n; i++)
        {
            sign *= -1;
            Real x = Real{std::cos(i * Numeric::pi / (n - 1))};
            interp.m_x.emplace_back(x);
            interp.m_f.emplace_back(f(x));
            interp.m_w.emplace_back(sign);
        }
        interp.m_w.front() *= 0.5;
        interp.m_w.back() *= 0.5;
        return interp;
    }

  protected:
    std::vector<Real> m_x; // 自变量
    std::vector<T> m_f;    // 因变量
    std::vector<Real> m_w; // 权重
};

template <class Real> struct Alg_CubicSpline_Interpolator
{
    enum BoundaryCondition
    {
        Complete,
        Natural,
        NotAKnot
    };

    /**
     * @brief 三弯矩插值
     *
     * @param[in] lbc 左边界类型
     * @param[in] rbc 右边界类型
     * @param[in] ba
     * @param[in] bb
     * @return PiecewisePolynomial<Real>
     */
    PiecewisePolynomial<Real> interpolate(BoundaryCondition lbc = Natural, BoundaryCondition rbc = Natural, Real ba = 0,
                                          Real bb = 0) const
    {
        auto lambda = [&](std::size_t i) { return (x[i + 1] - x[i]) / (x[i + 1] - x[i - 1]); };
        auto f2 = [&](std::size_t i, std::size_t j) { return (f[i] - f[j]) / (x[i] - x[j]); };
        auto f3 = [&](std::size_t i, std::size_t j, std::size_t k) { return (f2(i, j) - f2(j, k)) / (x[i] - x[k]); };

        auto N = x.size();
        Tridiagonal tridiag(N);

        MatrixXd b(1, N);
        for (std::size_t i = 1; i < N - 1; i++)
        {
            Real lam = lambda(i);
            tridiag(i, i + 1) = 1 - lam;
            tridiag(i, i) = 2;
            tridiag(i, i - 1) = lam;
            b(0, i) = f3(i - 1, i, i + 1) * 6;
        }

        switch (lbc)
        {
        case Complete: {
            tridiag(0, 0) = 2;
            tridiag(0, 1) = 1;
            b(0, 0) = (ba - f2(0, 1)) / (x[0] - x[1]) * 6;
            break;
        }
        case Natural: {
            tridiag(0, 0) = 1;
            b(0, 0) = ba;
            break;
        }
        case NotAKnot: {
            tridiag(0, 0) = 6;
            tridiag(0, 1) = 2;
            tridiag(1, 0) = tridiag(1, 2) = 0;
            tridiag(1, 1) = 3;

            b(0, 0) = b(0, 1);
            break;
        }
        }

        switch (rbc)
        {
        case Complete: {
            tridiag(N - 1, N - 1) = 2;
            tridiag(N - 1, N - 2) = 1;
            b(0, N - 1) = (f2(N - 2, N - 1) - bb) / (x[N - 2] - x[N - 1]) * 6;
            break;
        }
        case Natural: {
            tridiag(N - 1, N - 1) = 1;
            b(0, N - 1) = bb;
            break;
        }
        case NotAKnot: {
            tridiag(N - 1, N - 1) = 6;
            tridiag(N - 1, N - 2) = 2;
            tridiag(N - 2, N - 1) = tridiag(N - 2, N - 3) = 0;
            tridiag(N - 2, N - 2) = 3;

            b(0, N - 1) = b(0, N - 2);
            break;
        }
        }

        Matrix_Thomas_Solver solver(std::move(tridiag));
        solver.perform();
        auto M = solver.solve(std::move(b));

        // Not A Knot 的解进行了修正：K = (lambda_2 * M_1 + mu_2 * M_3) / 6
        if (lbc == NotAKnot)
        {
            Real lam1 = lambda(1), lam2 = lambda(N - 2);
            M(0, 0) = (6 * M(0, 0) - (1 - lam1) * M(0, 2)) / lam1;
        }

        if (rbc == NotAKnot)
        {
            Real lam1 = lambda(1), lam2 = lambda(N - 2);
            M(0, N - 1) = (6 * M(0, N - 1) - (1 - lam2) * M(0, N - 3)) / lam2;
        }

        // 使用 fi + mi(x - xi) + Mi / 2 * (x - xi)^2 + fi''' / 6 * (x - xi)^3 的形式代替 f(x) = a + bx + cx^2 +
        // dx^3 这样 fi 可以直接使用节点值，mi 和 fi''' 可以通过 Mi 直接计算 f(x) = a + bx + cx^2 + dx^3 f(x0) = f0,
        // f(x1) = f1, f''(x0) = M0, f''(x1) = M1
        // 优化后还是很慢，如果需要加速，需要使用更轻量的数据结构而不是笨重的多项式
        std::vector<Polynomial<Real>> polys;
        polys.reserve(N + 1);
        polys.emplace_back(0);
        for (std::size_t i = 0; i < N - 1; i++)
        {
            std::vector<Real> abcd(4);
            auto h = x[i + 1] - x[i];
            abcd[0] = f[i];
            abcd[1] = (f[i + 1] - f[i]) / h - (M(0, i + 1) + M(0, i) * 2) * h / 6;
            abcd[2] = M(0, i) / 2;
            abcd[3] = (M(0, i + 1) - M(0, i)) / (h * 6);
            polys.emplace_back(std::move(abcd));
            polys.back() >> x[i];
        }
        polys.emplace_back(polys.back());

        auto intervals = x;
        return PiecewisePolynomial<Real>{std::move(intervals), std::move(polys)};
    }

    std::vector<Real> x; // 自变量
    std::vector<Real> f; // 因变量
};

template <std::size_t N> struct Alg_CardinalSpline_Interpolator
{
    using Vec = VecT<N, double>; // P

    enum Degree
    {
        Cardinal2,
        Cardinal3
    };

    /**
     * @brief 基数样条插值
     * Cardinal2: 在 1.5, 2.5, 3.5, 4.5,... , n + 0.5 处插值，边界条件 S(1) = f(1), S(n + 1) = f(n + 1)
     * Cardinal3: 在 1, 2, 3, 4,... , n 处插值，边界条件 S'(1) = f'(1), S'(n) = f'(n)
     *
     * @param[in] d
     * @param[in] ba
     * @param[in] bb
     * @return std::shared_ptr<Geom_BSplineCurve<N>>
     */
    std::shared_ptr<Geom_BSplineCurve<N>> interpolate(Degree d, Vec ba, Vec bb) const
    {
        if (d == Cardinal2)
        {
            auto n = f.size();
            Tridiagonal tridiag(n);

            MatrixXd b(N, n);
            for (std::size_t i = 1; i < n - 1; i++)
            {
                tridiag(i, i + 1) = 1;
                tridiag(i, i) = 6;
                tridiag(i, i - 1) = 1;

                for (std::size_t j = 0; j < N; j++)
                    b(j, i) = f[i][j] * 8;
            }
            tridiag(0, 0) = tridiag(n - 1, n - 1) = 5;
            tridiag(0, 1) = tridiag(n - 1, n - 2) = 1;

            for (std::size_t j = 0; j < N; j++)
            {
                b(j, 0) = f[0][j] * 8 - ba[j] * 2;
                b(j, n - 1) = f[n - 1][j] * 8 - bb[j] * 2;
            }

            Matrix_Thomas_Solver solver(std::move(tridiag));
            solver.perform();

            auto a = solver.solve(std::move(b));
            std::vector<Vec> cpts(n + 2);
            for (std::size_t i = 1; i < n + 1; i++)
                for (std::size_t j = 0; j < N; j++)
                    cpts[i][j] = a(j, i - 1);

            for (std::size_t j = 0; j < N; j++)
            {
                cpts[0][j] = ba[j] * 2 - a(j, 0);
                cpts[n + 1][j] = bb[j] * 2 - a(j, n - 1);
            }

            // 节点向量 -1, 0, 1, ...
            std::vector<double> knots(n + 5);
            std::iota(knots.begin(), knots.end(), -1);

            return std::make_shared<Geom_BSplineCurve<N>>(std::move(cpts), std::move(knots), 2);
        }
        else
        {
            auto n = f.size();
            Tridiagonal tridiag(n);

            MatrixXd b(N, n);
            for (std::size_t i = 1; i < n - 1; i++)
            {
                tridiag(i, i + 1) = 1;
                tridiag(i, i) = 4;
                tridiag(i, i - 1) = 1;

                for (std::size_t j = 0; j < N; j++)
                    b(j, i) = f[i][j] * 6;
            }
            tridiag(0, 0) = tridiag(n - 1, n - 1) = 2;
            tridiag(0, 1) = tridiag(n - 1, n - 2) = 1;

            for (std::size_t j = 0; j < N; j++)
            {
                b(j, 0) = f[0][j] * 3 + ba[j];
                b(j, n - 1) = f[n - 1][j] * 3 - bb[j];
            }

            Matrix_Thomas_Solver solver(std::move(tridiag));
            solver.perform();

            auto a = solver.solve(std::move(b));
            std::vector<Vec> cpts(n + 2);
            for (std::size_t i = 1; i < n + 1; i++)
                for (std::size_t j = 0; j < N; j++)
                    cpts[i][j] = a(j, i - 1);

            for (std::size_t j = 0; j < N; j++)
            {
                cpts[0][j] = a(j, 1) - ba[j] * 2;
                cpts[n + 1][j] = a(j, n - 2) + bb[j] * 2;
            }

            // 节点向量 -2, -1, 0, 1, 2, ...
            std::vector<double> knots(n + 6);
            std::iota(knots.begin(), knots.end(), -2);

            return std::make_shared<Geom_BSplineCurve<N>>(std::move(cpts), std::move(knots), 3);
        }
    }

    std::vector<Vec> f; // 因变量
};

template <std::size_t N> struct Alg_Bezier_Interpolator
{
    using Vec = VecT<N, double>; // P

    /**
     * @brief 贝塞尔曲线插值
     *
     * @return std::shared_ptr<Geom_BezierCurve<N>>
     */
    std::shared_ptr<Geom_BezierCurve<N>> interpolate() const
    {
        std::size_t n = f.size() - 1;

        // 填系数矩阵
        MatrixXd M(n + 1, n + 1);
        for (std::size_t i = 0; i < n + 1; i++)
            for (std::size_t j = 0; j < n + 1; j++)
                M(i, j) = bernstein(n, j, x[i]);

        // 填充 Q
        MatrixXd Q(N, n + 1);
        for (std::size_t i = 0; i < n + 1; i++)
            for (std::size_t j = 0; j < N; j++)
                Q(j, i) = f[i][j];

        Matrix_PTLU_Solver solver(std::move(M));
        solver.perform();
        auto P = solver.solve(std::move(Q));

        // 构造 Bezier 曲线
        std::vector<Vec> cpts(n + 1);
        for (std::size_t i = 0; i < n + 1; i++)
            for (std::size_t j = 0; j < N; j++)
                cpts[i][j] = P(j, i);

        return std::make_shared<Geom_BezierCurve<N>>(std::move(cpts), n);
    }

    std::vector<double> x; // 自变量
    std::vector<Vec> f;    // 因变量
};

template <std::size_t N> struct Alg_Hermite_Interpolator
{
    using Vec = VecT<N, double>; // P

    /**
     * @brief 点法式几何 Hermite 插值
     *
     * @param[in] Q0
     * @param[in] Q1
     * @param[in] n0
     * @param[in] n1
     * @return std::shared_ptr<Geom_BezierCurve<N>>
     */
    static std::shared_ptr<Geom_BezierCurve<N>> interpolate(Vec Q0, Vec Q1, Vec n0, Vec n1)
    {
        double l = Q0.distance(Q1) * 0.4;
        Vec T0 = orthogonal_vec(n0);
        Vec T1 = orthogonal_vec(n1);
        std::vector<Vec> cpts(4);
        cpts[0] = Q0;
        cpts[1] = Q0 + T0 * l;
        cpts[2] = Q1 - T1 * l;
        cpts[3] = Q1;
        return std::make_shared<Geom_BezierCurve<N>>(std::move(cpts), 3);
    }
};

template <std::size_t N> struct Alg_ClassABezier_Interpolator
{
    static_assert(N == 3, "Only support 3D");
    using Vec = VecT<N, double>; // P

    /**
     * @brief 计算类 A 贝塞尔曲线的控制点
     * 给定切向序列： Ti = M^i * T0, i = 0, 1, 2,..., n
     * 控制点序列：0, T0, T0 + T1, T0 + T1 + T2,..., T0 + T1 +... + Tn
     * M = (s)^(1/n) * R，其中 R 为旋转矩阵，s 为缩放因子
     *
     * @param[in] T0
     * @param[in] Tn
     * @return std::shared_ptr<Geom_BezierCurve<N>>
     */
    static std::shared_ptr<Geom_BezierCurve<N>> interpolate(Vec T0, Vec Tn)
    {
        // 要求 Tn 长度大于 T0 长度
        double r = Tn.modulus() / T0.modulus();
        if (r < 1 + std::numeric_limits<double>::epsilon())
            return nullptr;

        double s = std::sqrt(r);
        double angle = std::acos(dot(T0, Tn) / (T0.modulus() * Tn.modulus()));

        int n = 2;
        while (std::cos(angle / n) <= 1 / s)
        {
            // 这会让 n 取得更大，s 变小，当循环足够多时，上面等式会成立
            n = std::ceil(angle / std::acos(1 / s));
            s = std::pow(r, 1.0 / n);
        }

        // T0 转动 n 次，得到 n + 1 个切向
        auto q = Quat<double>::rotate(cross(T0, Tn), angle / n);
        Quat<double> p(0.0, T0);

        // 从 0 开始计算 n + 2 个控制点
        std::vector<Vec> cpts(n + 2);
        for (std::size_t i = 1; i < n + 2; i++)
        {
            cpts[i] = cpts[i - 1] + p.v;
            p = q * p / q * s;
        }

        return std::make_shared<Geom_BezierCurve<N>>(std::move(cpts), n + 1);
    }
};

template <std::size_t N> struct Alg_Kappa_Interpolator
{
};

template <std::size_t N> struct Alg_CubicBSpline_Interpolator
{
    using Vec = VecT<N, double>; // P

    enum KnotType
    {
        Parameter, // 使用参数值作为节点
        Average,   // 使用参数平均值作为节点
    };

    enum BoundaryCondition
    {
        Complete,
        Natural
    };

    /**
     * @brief 三次 B 样条插值：允许指定边界条件，因此比一般的插值多 2 个控制点
     * 对于 Complete 边界条件，如果只给定方向，则模长最好取总弦长
     *
     * @param[in] f 数据点
     * @param[in] lbc 左边界类型
     * @param[in] rbc 右边界类型
     * @param[in] ba
     * @param[in] bb
     * @return std::shared_ptr<Geom_BSplineCurve<N>>
     */
    static std::shared_ptr<Geom_BSplineCurve<N>> interpolate(std::vector<Vec> f, BoundaryCondition lbc = Natural,
                                                             BoundaryCondition rbc = Natural, Vec ba = Vec{},
                                                             Vec bb = Vec{})
    {
        auto params = Alg_Parameter<N>::chord(f);

        std::size_t p = 3;
        std::size_t K = f.size() - 1;
        std::vector<double> knots(K + 1 + p * 2);
        for (std::size_t i = 0; i < p; i++)
            knots[i] = params[0];
        for (std::size_t i = p; i < K + p + 1; i++)
            knots[i] = params[i - p];
        for (std::size_t i = K + p + 1; i < knots.size(); i++)
            knots[i] = params[K];

        Tridiagonal tridiag(K + 1);
        for (std::size_t i = 1; i < K; i++)
        {
            auto basis = basis_funcs(knots.data(), p, i + p, params[i]);
            tridiag(i, i - 1) = basis[0];
            tridiag(i, i) = basis[1];
            tridiag(i, i + 1) = basis[2];
        }

        MatrixXd Q(N, K + 1);
        for (std::size_t i = 1; i < K; i++)
        {
            for (std::size_t j = 0; j < N; j++)
                Q(j, i) = f[i][j];
        }

        if (lbc == BoundaryCondition::Complete)
        {
            tridiag(0, 0) = 3 / (params[1] - params[0]);
            auto Q0 = f[0] * 3 / (params[1] - params[0]) + ba;
            for (std::size_t j = 0; j < N; j++)
                Q(j, 0) = Q0[j];
        }
        else if (lbc == BoundaryCondition::Natural)
        {
            tridiag(0, 0) = -2 * (params[1] - params[0]) - (params[2] - params[1]);
            tridiag(0, 1) = params[1] - params[0];
            double coeff = (params[2] - params[0]) * std::pow(params[1] - params[0], 2) / 6;
            auto Q0 = ba * coeff - f[0] * (params[2] - params[0]);
            for (std::size_t j = 0; j < N; j++)
                Q(j, 0) = Q0[j];
        }

        if (rbc == BoundaryCondition::Complete)
        {
            tridiag(K, K) = 3 / (params[K] - params[K - 1]);
            auto QK = f[K] * 3 / (params[K] - params[K - 1]) - bb;
            for (std::size_t j = 0; j < N; j++)
                Q(j, K) = QK[j];
        }
        else if (rbc == BoundaryCondition::Natural)
        {
            tridiag(K, K) = -2 * (params[K] - params[K - 1]) - (params[K - 1] - params[K - 2]);
            tridiag(K, K - 1) = params[K] - params[K - 1];
            double coeff = (params[K] - params[K - 2]) * std::pow(params[K] - params[K - 1], 2) / 6;
            auto QK = bb * coeff - f[K] * (params[K] - params[K - 2]);
            for (std::size_t j = 0; j < N; j++)
                Q(j, K) = QK[j];
        }

        Matrix_Thomas_Solver solver(std::move(tridiag));
        solver.perform();
        auto P = solver.solve(std::move(Q));

        std::vector<Vec> cpts(K + 3);
        cpts[0] = f[0];
        for (std::size_t i = 0; i < K + 1; i++)
            for (std::size_t j = 0; j < N; j++)
                cpts[i + 1][j] = P(j, i);
        cpts[K + 2] = f[K];

        return std::make_shared<Geom_BSplineCurve<N>>(std::move(cpts), std::move(knots), 3);
    }

    /**
     * @brief 三次 B 样条插值
     *
     * @param[in] f
     * @return std::shared_ptr<Geom_BSplineSurface<N>>
     */
    static std::shared_ptr<Geom_BSplineSurface<N>> interpolate(std::vector<std::vector<Vec>> f)
    {
        std::size_t nu = f.size();
        std::size_t nv = f.front().size();

        std::vector<double> uknots;
        std::vector<double> vknots;

        // 插值后翻转方向，便于在另一个方向插值
        // 注意每次插值以后都会多 2 个控制点
        std::vector<std::vector<Vec>> tar2(nv + 2, std::vector<Vec>(nu));
        for (std::size_t i = 0; i < nu; i++)
        {
            auto spline = interpolate(f[i]);
            for (std::size_t j = 0; j < nv + 2; j++)
                tar2[j][i] = spline->pole(j);

            if (i == 0)
                vknots = std::move(spline->move_knots());
        }

        std::vector<Vec> tar3((nu + 2) * (nv + 2));
        for (std::size_t i = 0; i < nv + 2; i++)
        {
            auto spline = interpolate(tar2[i]);
            for (std::size_t j = 0; j < nu + 2; j++)
                tar3[j * (nv + 2) + i] = spline->pole(j);

            if (i == 0)
                uknots = std::move(spline->move_knots());
        }

        return std::make_shared<Geom_BSplineSurface<N>>(std::move(tar3), std::move(uknots), std::move(vknots), 3, 3);
    }
};

} // namespace alg

} // namespace xi
